#include <irrlicht.h>
#include <btBulletDynamicsCommon.h>
#include "SEMisc.h"


class MotionState : public btMotionState
{
    
public:
    
    MotionState(const btTransform& initalTransformation, irr::scene::ISceneNode* const node) :
    node(node), initalTransformation(initalTransformation)
    {
        
    }
    
    void getWorldTransform(btTransform& worldTrans) const
    {
        worldTrans = this->initalTransformation;
    }
    
    void setWorldTransform(const btTransform& worldTrans)
    {
        btScalar btMat[16];
        irr::f32 irrMat[16];
        
        worldTrans.getOpenGLMatrix(btMat);
        
        for (int i=0; i<16; i++) {
            irrMat[i] = btMat[i];
        }
        
        matr.setM(irrMat);
        
        vec3f pos = matr.getTranslation();
        
        this->node->setRotation(matr.getRotationDegrees());
        this->node->setPosition(matr.getTranslation());
    }
    
private:
    
    irr::scene::ISceneNode* const node;
    
    irr::core::matrix4 matr;
    
    btTransform initalTransformation;
};


int main1(int argc, char** arg) {
    irr::IrrlichtDevice* device = irr::createDevice();
    irr::video::IVideoDriver* driver = device->getVideoDriver();
    irr::scene::ISceneManager* smgr = device->getSceneManager();
    
    
    btDbvtBroadphase* broadphase = new btDbvtBroadphase;
    btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration;
    btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
    btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
    
    btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
    dynamicsWorld->setGravity(btVector3(0, -9.80665, 0));
    
    
    irr::scene::ICameraSceneNode* camera = smgr->addCameraSceneNode();
    camera->setPosition(irr::core::vector3df(0.0f, 0.0f, -5.0f));
    camera->bindTargetAndRotation(true);
    
    ////////////////////////////////////////////////////////////////////////////
    //
    //              CREATION DU SOL
    //
    ////////////////////////////////////////////////////////////////////////////
    irr::scene::IMeshSceneNode* ground = smgr->addCubeSceneNode(10.0f,
                                                                NULL,
                                                                -1,
                                                                irr::core::vector3df(0, 0, 0),
                                                                irr::core::vector3df(0, 0, 0),
                                                                irr::core::vector3df(10.0f, 1.0f, 10.0f));
    std::string tex_file = "/Users/ydethe/Documents/XCode-Projects/Librairies/irrlicht-1.8/media/rockwall.jpg";
    ground->setMaterialTexture(0, driver->getTexture(tex_file.c_str()));
    ground->setMaterialFlag(irr::video::EMF_LIGHTING, false);
    
    btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0.0, 1.0, 0.0), 1.0);
    MotionState* groundMotionState = new MotionState(btTransform(btQuaternion(0, 0, 0, 1),btVector3(0,-10,0)), ground);
    
    btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0.0, 0.0, 0.0));
    btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI);
    dynamicsWorld->addRigidBody(groundRigidBody);
        
    
    ////////////////////////////////////////////////////////////////////////////
    //
    //              CREATION DE LA BALLE
    //
    ////////////////////////////////////////////////////////////////////////////
    irr::scene::IMeshSceneNode* sphere = smgr->addSphereSceneNode(1.0f);
    tex_file = "/Users/ydethe/Documents/XCode-Projects/Librairies/irrlicht-1.8/media/earth.jpg";
    sphere->setMaterialTexture(0, driver->getTexture(tex_file.c_str()));
    sphere->setMaterialFlag(irr::video::EMF_LIGHTING, false);
    
    btCollisionShape* fallShape = new btSphereShape(1.0);
    MotionState* fallMotionState = new MotionState(btTransform(btQuaternion(0.0, 0.0, 0.0, 1.0), btVector3(0.0, 50.0, 0.0)), sphere);
    btScalar mass = 1.0;
    btVector3 inertia(0.0, 0.0, 0.0);
    fallShape->calculateLocalInertia(mass, inertia);
    
    btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass, fallMotionState, fallShape, inertia);
    btRigidBody* fallRigidBody = new btRigidBody(fallRigidBodyCI);
    dynamicsWorld->addRigidBody(fallRigidBody);
    
    
    ////////////////////////////////////////////////////////////////////////////
    //
    //              PREPARATION DE LA BOUCLE PRINCIPALE
    //
    ////////////////////////////////////////////////////////////////////////////
    irr::ITimer* const timer = device->getTimer();
    irr::u32 then = timer->getTime();
    
    while (device->run()) {
        const irr::u32 now = timer->getTime();
        const btScalar frameDeltaTime = (btScalar)(now - then)*0.001; // Time in seconds
        then = now;
        
        dynamicsWorld->stepSimulation(frameDeltaTime, 10);
        
        btVector3 pos = fallRigidBody->getCenterOfMassPosition();
        std::cout << pos.x() << ", " << pos.y() << ", " << pos.z() << std::endl;
        
        vec3f irr_pos(pos.x(), pos.y(), pos.z());
        camera->setTarget(irr_pos);
        
        driver->beginScene(true, true, irr::video::SColor(255, 255, 255, 255));
        smgr->drawAll();
        driver->endScene();
        
    }
    
    
    // Clean up behind ourselves like good little programmers
    dynamicsWorld->removeRigidBody(fallRigidBody);
    delete fallRigidBody->getMotionState();
    delete fallRigidBody;
    
    dynamicsWorld->removeRigidBody(groundRigidBody);
    delete groundRigidBody->getMotionState();
    delete groundRigidBody;
    
    delete fallShape;
    delete groundShape;
    
    delete dynamicsWorld;
    delete solver;
    delete dispatcher;
    delete collisionConfiguration;
    delete broadphase;
    
    device->drop();
    
    return 0;
}
